#include "FOpenDriveImporter.h"
#include "../pugixml/pugixml.hpp"
#include "../WorldException.h"

namespace ODRoadGeom
{
    FOpenDriveImporter::FOpenDriveImporter(const wchar_t* filename, double unitscale, ODSystem& OpenDriveSystem)
        : UnitScale(unitscale)
    {
        pugi::xml_document  fileSource;
        pugi::xml_parse_result result = fileSource.load_file(filename);
        if (!result)
        {
            throw FWorldException(result.description());
        }

        pugi::xml_node  root = fileSource.child("OpenDRIVE");
        ImportRoot(root, OpenDriveSystem);
    }

    void        FOpenDriveImporter::ImportRoot(const pugi::xml_node& XmlRootNode , ODSystem& OpenDriveSystem )
    {
        if (XmlRootNode.empty())
        {
            return;
        }
        // load information
        for (auto& road : XmlRootNode.children("road"))
        {
            ImportRoad(road, OpenDriveSystem);
        }
        for (auto& ctrl : XmlRootNode.children("controller"))
        {
            ImportController(ctrl, OpenDriveSystem);
        }
        for (auto& junction : XmlRootNode.children("junction"))
        {
            ImportJunction(junction, OpenDriveSystem);
        }


        // parse links
        for (auto& XmlNode : XmlRootNode.children("road"))
        {
            std::string id = XmlNode.attribute("id").as_string();

            ODRoadPtr road = OpenDriveSystem.GetRoad(id);

            ImportRoadLink(road, XmlNode.child("link"));       
        }
    }

    // import road
    void        FOpenDriveImporter::ImportRoad(const pugi::xml_node& XmlRoadNode, ODSystem& OpenDriveSystem)
    {
        std::string id = XmlRoadNode.attribute("id").as_string();
        std::string name = XmlRoadNode.attribute("name").as_string();
        
        ODRoadPtr odRoad(new ODRoad(id, name));
        odRoad->length = XmlRoadNode.attribute("length").as_double() * UnitScale;            
        odRoad->junctionId = XmlRoadNode.attribute("junction").as_string();

        // import road  
        ImportPlanView(odRoad,  XmlRoadNode.child("planView") );
        ImportElevation(odRoad, XmlRoadNode.child("elevationProfile"));
        ImportLaneGroup(odRoad, XmlRoadNode.child("lanes"));
        ImportSignals(odRoad, XmlRoadNode.child("signals"), OpenDriveSystem);
        OpenDriveSystem.roadMap[odRoad->id] = odRoad;
    }

    void        FOpenDriveImporter::ImportRoadLink(ODRoadPtr road, const pugi::xml_node& xmlNode )
    {
        if (xmlNode.empty())
            return;

        pugi::xml_node predecessor = xmlNode.child("predecessor");
        pugi::xml_node successor = xmlNode.child("successor");
        {
            if (!predecessor.empty())
            {
                std::string typestring = predecessor.attribute("elementType").as_string();
                std::string eleId = predecessor.attribute("elementId").as_string();
                std::string contactPoint = predecessor.attribute("contactPoint").as_string();

                int dir = 1;
                if (contactPoint == std::string("end"))
                {
                    dir = -1;
                }
                ODLink::LinkType linkType = ODLink::ELT_UNKNOWN;
                if (typestring == std::string("road"))
                {
                    linkType = ODLink::ELT_ROAD;
                }
                else if (typestring == std::string("junction"))
                {
                    linkType = ODLink::ELT_JUNCTION;
                }

                ODLinkPtr p(new ODLink(linkType, eleId, dir));
                road->predecessor = p;
            }
            if (!successor.empty())
            {
                std::string typestring = successor.attribute("elementType").as_string();
                std::string eleId = successor.attribute("elementId").as_string();
                std::string contactPoint = successor.attribute("contactPoint").as_string();

                int dir = 1;
                if (contactPoint == std::string("end"))
                {
                    dir = -1;
                }
                ODLink::LinkType linkType = ODLink::ELT_UNKNOWN;
                if (typestring == std::string("road"))
                {
                    linkType = ODLink::ELT_ROAD;
                }
                else if (typestring == std::string("junction"))
                {
                    linkType = ODLink::ELT_JUNCTION;
                }

                ODLinkPtr p(new ODLink(linkType, eleId, dir));
                road->successor = p;
            }
        }
    }


    void        FOpenDriveImporter::ImportPlanView(ODRoadPtr road, const pugi::xml_node& xmlNode)
    {
        for (auto& node : xmlNode.children("geometry"))
        {            
            {
                double geometryStart = node.attribute("s").as_double() * UnitScale;
                double geometryX = (node.attribute("x")).as_double() * UnitScale;
                double geometryY = (node.attribute("y")).as_double() * UnitScale;
                double geometryHdg = (node.attribute("hdg")).as_double();
                double geometryLength = (node.attribute("length")).as_double() * UnitScale;

                pugi::xml_node XmlLineNode = node.child(("line"));
                pugi::xml_node XmlSpiralNode = node.child(("spiral"));
                pugi::xml_node XmlArcNode = node.child(("arc"));
                pugi::xml_node XmlPoly3Node = node.child(("poly3"));
                pugi::xml_node XmlParamPoly3Node = node.child(("paramPoly3"));

                if (XmlLineNode)
                {
                    road->AddPlanLine(geometryStart, geometryX, geometryY, geometryHdg, geometryLength);
                }
                else if (XmlSpiralNode)
                {
                    double cs = (XmlSpiralNode.attribute("curvStart")).as_double() * (1 / UnitScale);
                    double ce = (XmlSpiralNode.attribute("curvEnd")).as_double() * (1 / UnitScale);
                    road->AddPlanSpiral(geometryStart, geometryX, geometryY, geometryHdg, geometryLength, cs, ce);
                }
                else if (XmlArcNode)
                {
                    double cr = (XmlArcNode.attribute("curvature")).as_double()* (1 / UnitScale);
                    road->AddPlanArc(geometryStart, geometryX, geometryY, geometryHdg, geometryLength, cr);
                }
                else if (XmlPoly3Node)
                {
                    double a = (XmlPoly3Node.attribute("a")).as_double() * UnitScale;
                    double b = (XmlPoly3Node.attribute("b")).as_double();
                    double c = (XmlPoly3Node.attribute("c")).as_double()* (1 / UnitScale);
                    double d = (XmlPoly3Node.attribute("d")).as_double()* (1 / UnitScale) * (1 / UnitScale);
                    road->AddPlanPoly3(geometryStart, geometryX, geometryY, geometryHdg, geometryLength, a, b, c, d);

                }
                else if (XmlParamPoly3Node)
                {
                    double aU = (XmlParamPoly3Node.attribute("aU")).as_double()* UnitScale;
                    double bU = (XmlParamPoly3Node.attribute("bU")).as_double();
                    double cU = (XmlParamPoly3Node.attribute("cU")).as_double()* (1 / UnitScale);
                    double dU = (XmlParamPoly3Node.attribute("dU")).as_double()* (1 / UnitScale)* (1 / UnitScale);
                    double aV = (XmlParamPoly3Node.attribute("aV")).as_double()* UnitScale;
                    double bV = (XmlParamPoly3Node.attribute("bV")).as_double();
                    double cV = (XmlParamPoly3Node.attribute("cV")).as_double()* (1 / UnitScale);
                    double dV = (XmlParamPoly3Node.attribute("dV")).as_double()* (1 / UnitScale)* (1 / UnitScale);
                    string sRange = (XmlParamPoly3Node.attribute("pRange")).as_string();
                    double pRange = 1.0f;
                    if (sRange == "arcLength")
                    {
                        pRange = geometryLength;
                    }
                    road->AddParamPoly3(geometryStart, geometryX, geometryY, geometryHdg, geometryLength, aU, bU, cU, dU, aV, bV, cV, dV, pRange);
                }

            }
        }
    }


    void        FOpenDriveImporter::ImportElevation(ODRoadPtr road, const pugi::xml_node& xmlNode)
    {
        if (xmlNode.empty())
            return;

        for (auto& node : xmlNode.children("elevation"))
        {

            double s = (node.attribute("s")).as_double()* UnitScale;
            double a = (node.attribute("a")).as_double()* UnitScale;
            double b = (node.attribute("b")).as_double();
            double c = (node.attribute("c")).as_double()* (1 / UnitScale);
            double d = (node.attribute("d")).as_double()* (1 / UnitScale)* (1 / UnitScale);
            road->AddElevation(s, a, b, c, d);
        }
    }

    void        FOpenDriveImporter::ImportLaneGroup(ODRoadPtr road, const pugi::xml_node& xmlNode)
    {
        if (xmlNode.empty())
            return;

        for (auto& node : xmlNode.children("laneSection"))
        {
            double s = (node.attribute("s")).as_double() * UnitScale;
            ODLaneSectionPtr ls(new ODLaneSection(s));
            ImportLaneSection(ls, node);
            road->AddLaneSection(ls);         
        }
    }


    void        FOpenDriveImporter::ImportLaneSection(ODLaneSectionPtr ls, const pugi::xml_node& xmlNode)
    {
        if (xmlNode.empty())
            return;

        pugi::xml_node left = xmlNode.child(("left"));
        ImportLane(ls, left);

        pugi::xml_node center = xmlNode.child(("center"));
        ImportLane(ls, center);

        pugi::xml_node right = xmlNode.child(("right"));
        ImportLane(ls, right);
    }


    void        FOpenDriveImporter::ImportLane(ODLaneSectionPtr ls, const pugi::xml_node& xmlNode)
    {
        if (xmlNode.empty())
            return;

        for ( auto&node : xmlNode.children("lane"))
        {           
            int id = node.attribute("id").as_int();
            std::string typestring = (node.attribute("type").as_string());
            std::string level = (node.attribute("level")).as_string();
            ODLanePtr lane(new ODLane(id, typestring, level));
            ImportLaneDetail(lane, node);
            ls->AddLane(lane);
        }
    }


    void        FOpenDriveImporter::ImportLaneDetail(ODLanePtr lane, const pugi::xml_node& xmlNode)
    {
        pugi::xml_node widthnode = xmlNode.child(("width"));
        pugi::xml_node hightnode = xmlNode.child(("height"));
        pugi::xml_node roadMarknode = xmlNode.child(("roadMark"));
        pugi::xml_node link = xmlNode.child(("link"));

        if (!widthnode.empty())
        {
            double s = (widthnode.attribute("sOffset").as_double())* UnitScale;
            double a = (widthnode.attribute("a").as_double())* UnitScale;
            double b = (widthnode.attribute("b").as_double());
            double c = (widthnode.attribute("c").as_double())* (1 / UnitScale);
            double d = (widthnode.attribute("d").as_double())* (1 / UnitScale)* (1 / UnitScale);
            lane->AddWidth(s, a, b, c, d);
        }
        if (!hightnode.empty())
        {
            double s = (hightnode.attribute("sOffset").as_double())* UnitScale;
            double ier = (hightnode.attribute("inner").as_double())* UnitScale;
            if (ier == 0.0)
            {
                ier = (hightnode.attribute("heightInner").as_double())* UnitScale;
            }
            double oer = (hightnode.attribute("outer").as_double())* UnitScale;
            if (oer == 0.0)
            {
                oer = (hightnode.attribute("heightOuter").as_double())* UnitScale;
            }
            lane->AddHeight(s, ier, oer);
        }
        if (!roadMarknode.empty())
        {
            double s = (roadMarknode.attribute("sOffset").as_double())* UnitScale;
            std::string type = (roadMarknode.attribute("type").as_string());
            std::string weight = (roadMarknode.attribute("weight").as_string());
            std::string color = (roadMarknode.attribute("color").as_string());
            std::string material = (roadMarknode.attribute("material").as_string());
            double width = (roadMarknode.attribute("width").as_double())* UnitScale;
            std::string laneChange = (roadMarknode.attribute("laneChange").as_string());
            double height = (roadMarknode.attribute("height").as_double())* UnitScale;
            lane->AddRoadMark(s, type, weight, color, material, width, laneChange, height);
        }
        if (!link.empty())
        {
            pugi::xml_node predecessor = link.child(("predecessor"));
            pugi::xml_node successor = link.child(("successor"));
            if ( !predecessor.empty() )
                lane->predecessor = (predecessor.attribute("id").as_int());
            if (!successor.empty())
                lane->successor = (successor.attribute("id").as_int());
        }
        
    }

    void        FOpenDriveImporter::ImportSignals(ODRoadPtr road, const pugi::xml_node& xmlNode, ODSystem& OpenDriveSystem)
    {
        if (xmlNode.empty())
            return;

        for (auto& node : xmlNode.children("signal"))
        {
            double s = node.attribute("s").as_double() * UnitScale;
            double t = node.attribute("t").as_double()* UnitScale;
            std::string id = node.attribute("id").as_string();
            std::string name = node.attribute("name").as_string();
            std::string dynamic = node.attribute("dynamic").as_string();
            std::string orientation = node.attribute("orientation").as_string();
            double zOffset = node.attribute("zOffset").as_double()* UnitScale;
            std::string country = node.attribute("country").as_string();
            std::string type = node.attribute("type").as_string();
            std::string subtype = node.attribute("subtype").as_string();
            double value = node.attribute("value").as_double();
            std::string unit = node.attribute("unit").as_string();
            double height = node.attribute("height").as_double()* UnitScale;
            double width = node.attribute("width").as_double()* UnitScale;
            std::string text = node.attribute("text").as_string();
            double hOffset = node.attribute("hOffset").as_double();
            double pitch = node.attribute("pitch").as_double();
            double roll = node.attribute("roll").as_double();

            OpenDriveSystem.AddSignal(s, t, id, name, dynamic, orientation, zOffset, country, type, subtype, value, unit, height, width, text, hOffset, pitch, roll);
            road->AddSignalRef(s, t, id, orientation);


            //if (node->GetTag() == TEXT("signalReference"))
            //{
            //    double s = GetDoubleAttribute(node, TEXT("s"))* UnitScale;
            //    double t = GetDoubleAttribute(node, TEXT("t"))* UnitScale;
            //    std::string id = GetStringAttribute(node, TEXT("id"));
            //    std::string orientation = GetStringAttribute(node, TEXT("orientation"));
            //    road->AddSignalRef(s, t, id, orientation);
            //}
        }
    }

    // import Controller
    void        FOpenDriveImporter::ImportController(const pugi::xml_node& XmlRoadNode, ODSystem& OpenDriveSystem)
    {
        if (XmlRoadNode.empty())
            return;

        std::string id = (XmlRoadNode.attribute("id").as_string());
        std::string name = (XmlRoadNode.attribute("name").as_string());
        int32_t seq = (XmlRoadNode.attribute("sequence").as_int());

        ODControllerPtr odctrl(new ODController(id, name, seq));

        for (auto& XmlNode : XmlRoadNode.children("control"))
        {
            ImportControl(odctrl, XmlNode);         
        }


        OpenDriveSystem.ctrlMap[odctrl->id] = odctrl;
    }

    void        FOpenDriveImporter::ImportControl(ODControllerPtr odctrl, const pugi::xml_node& xmlNode)
    {
        if (xmlNode.empty())
            return;

        std::string signalId = xmlNode.attribute("signalId").as_string();
        std::string type = xmlNode.attribute("type").as_string();
        ODController::Control ctl(signalId, type);
        odctrl->controls.push_back(ctl);
    }

    // import junction
    void        FOpenDriveImporter::ImportJunction(const pugi::xml_node& XmlRoadNode, ODSystem& OpenDriveSystem)
    {
        if (XmlRoadNode.empty())
            return;
        std::string id = XmlRoadNode.attribute("id").as_string();
        std::string name = XmlRoadNode.attribute("name").as_string();

        ODJunctionPtr odJunction(new ODJunction(id, name));

        for (auto& node : XmlRoadNode.children("connection"))
        {
            std::string idconn =node.attribute("id").as_string();
            std::string incomingRoad =node.attribute("incomingRoad").as_string();
            std::string connectingRoad =node.attribute("connectingRoad").as_string();
            std::string contactPoint =node.attribute("contactPoint").as_string();

            ODConnectionPtr conn(new ODConnection(idconn, incomingRoad, connectingRoad, contactPoint));
            ImportConnection(conn, node);
            odJunction->AddConnection(conn);
        }
        for (auto& node : XmlRoadNode.children("controller"))
        {
            std::string idctrl =node.attribute("id").as_string();
            std::string type =node.attribute("type").as_string();
            odJunction->AddController(idctrl, type);
        }
        OpenDriveSystem.junctionMap[odJunction->id] = odJunction;
    }
    
    void        FOpenDriveImporter::ImportConnection(ODConnectionPtr conn, const pugi::xml_node& xmlNode)
    {
        if (xmlNode.empty())
            return;

        for (auto& node : xmlNode.children("laneLink"))
        {
            int32_t from = node.attribute("from").as_int();
            int32_t to = node.attribute("to").as_int();
            conn->AddLaneLink(from, to);            
        }
    }

}